/* ---------------------------------------------------------------------------
** This software is in the public domain, furnished "as is", without technical
** support, and with no warranty, express or implied, as to its usefulness for
** any purpose.
**
** V4l2Device.cpp
** 
** -------------------------------------------------------------------------*/

#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <fcntl.h>

// libv4l2
#include <linux/videodev2.h>
#include <linux/uvcvideo.h>
#include <sys/ioctl.h>
#include "logger.h"

#include "V4l2Device.h"


typedef enum{
	CHIP_NONE = -1,
	CHIP_RER9420 = 0,
	CHIP_RER9421,
	CHIP_RER9422
}CHIP_RER942X;

#define XU_RERVISION_SYS_ID							0x03
#define XU_RERVISION_SYS_H264_CTRL					0x07
#define XU_RERVISION_USR_ID							0x04
#define XU_RERVISION_USR_H264_CTRL					0x02
#define UVC_SET_CUR					0x01
#define UVCIOC_CTRL_QUERY	_IOWR('u', 0x21, struct uvc_xu_control_query)

int XU_Set_Cur(int fd, __u8 xu_unit, __u8 xu_selector, __u16 xu_size, __u8 *xu_data);
int XU_H264_Set_BitRate(int fd, double BitRate,int m_CurrentFPS);
// -----------------------------------------
//    V4L2Device
// -----------------------------------------
V4l2Device::V4l2Device(const V4L2DeviceParameters&  params, v4l2_buf_type deviceType) : m_params(params), m_fd(-1), m_deviceType(deviceType), m_bufferSize(0), m_format(0)
{
}

V4l2Device::~V4l2Device() 
{
	this->close();
}

void V4l2Device::close() 
{
	if (m_fd != -1) 		
		::close(m_fd);
	
	m_fd = -1;
}

// query current format
void V4l2Device::queryFormat()
{
	struct v4l2_format     fmt;
	memset(&fmt,0,sizeof(fmt));
	fmt.type  = m_deviceType;
	if (0 == ioctl(m_fd,VIDIOC_G_FMT,&fmt)) 
	{
		m_format     = fmt.fmt.pix.pixelformat;
		m_width      = fmt.fmt.pix.width;
		m_height     = fmt.fmt.pix.height;
		m_bufferSize = fmt.fmt.pix.sizeimage;
	}
}

// intialize the V4L2 connection
bool V4l2Device::init(unsigned int mandatoryCapabilities)
{
        struct stat sb;
        if ( (stat(m_params.m_devName.c_str(), &sb)==0) && ((sb.st_mode & S_IFMT) == S_IFCHR) )
        {
		if (initdevice(m_params.m_devName.c_str(), mandatoryCapabilities) == -1)
		{
			LOG(ERROR) << "Cannot init device:" << m_params.m_devName;
		}
	}
	else
	{
                // open a normal file
                m_fd = open(m_params.m_devName.c_str(), O_WRONLY | O_CREAT | O_TRUNC, S_IRWXU);
	}
	return (m_fd!=-1);
}

// intialize the V4L2 device
int V4l2Device::initdevice(const char *dev_name, unsigned int mandatoryCapabilities)
{
	m_fd = open(dev_name,  O_RDWR | O_NONBLOCK);
	if (m_fd < 0) 
	{
		LOG(ERROR) << "Cannot open device:" << m_params.m_devName << " " << strerror(errno);
		this->close();
		return -1;
	}
	if (checkCapabilities(m_fd,mandatoryCapabilities) !=0)
	{
		this->close();
		return -1;
	}	
	if (configureFormat(m_fd) !=0) 
	{
		this->close();
		return -1;
	}
	if (configureParam(m_fd) !=0)
	{
		this->close();
		return -1;
	}
	
	return m_fd;
}

// check needed V4L2 capabilities
int V4l2Device::checkCapabilities(int fd, unsigned int mandatoryCapabilities)
{
	struct v4l2_capability cap;
	memset(&(cap), 0, sizeof(cap));
	if (-1 == ioctl(fd, VIDIOC_QUERYCAP, &cap)) 
	{
		LOG(ERROR) << "Cannot get capabilities for device:" << m_params.m_devName << " " << strerror(errno);
		return -1;
	}
	LOG(NOTICE) << "driver:" << cap.driver << " capabilities:" << std::hex << cap.capabilities <<  " mandatory:" << mandatoryCapabilities << std::dec;
		
	if ((cap.capabilities & V4L2_CAP_VIDEO_OUTPUT))  LOG(NOTICE) << m_params.m_devName << " support output";
	if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) LOG(NOTICE) << m_params.m_devName << " support capture";

	if ((cap.capabilities & V4L2_CAP_READWRITE))     LOG(NOTICE) << m_params.m_devName << " support read/write";
	if ((cap.capabilities & V4L2_CAP_STREAMING))     LOG(NOTICE) << m_params.m_devName << " support streaming";

	if ((cap.capabilities & V4L2_CAP_TIMEPERFRAME))  LOG(NOTICE) << m_params.m_devName << " support timeperframe"; 
	
	if ( (cap.capabilities & mandatoryCapabilities) != mandatoryCapabilities )
	{
		LOG(ERROR) << "Mandatory capability not available for device:" << m_params.m_devName;
		return -1;
	}
	
	return 0;
}

std::string fourcc(unsigned int format)
{
	char formatArray[] = { (char)(format&0xff), (char)((format>>8)&0xff), (char)((format>>16)&0xff), (char)((format>>24)&0xff), 0 };
	return std::string(formatArray, strlen(formatArray));
}

// configure capture format 
int V4l2Device::configureFormat(int fd)
{
	
	if (m_params.m_formatList.size()==0) 
	{
		this->queryFormat();
		m_params.m_format = m_format;
		m_params.m_width  = m_width;
		m_params.m_height = m_height;
		
		LOG(NOTICE) << m_params.m_devName << ":" << fourcc(m_format) << " size:" << m_params.m_width << "x" << m_params.m_height;
		return 0;
	}		
	
	std::list<unsigned int>::iterator it;
	for (it = m_params.m_formatList.begin(); it != m_params.m_formatList.end(); ++it) {
		unsigned int format = *it;
		if (this->configureFormat(fd, format)==0) {
			return 0;
		}
	}
	return -1;
}

// configure capture format 
int V4l2Device::configureFormat(int fd, unsigned int format)
{
	struct v4l2_format   fmt;			
	memset(&(fmt), 0, sizeof(fmt));
	fmt.type                = m_deviceType;
	fmt.fmt.pix.width       = m_params.m_width;
	fmt.fmt.pix.height      = m_params.m_height;
	fmt.fmt.pix.pixelformat = format;
	fmt.fmt.pix.field       = V4L2_FIELD_ANY;
	
	if (ioctl(fd, VIDIOC_S_FMT, &fmt) == -1)
	{
		LOG(ERROR) << "Cannot set format for device:" << m_params.m_devName << " " << strerror(errno);
		return -1;
	}			
	if (fmt.fmt.pix.pixelformat != format) 
	{
		LOG(ERROR) << "Cannot set pixelformat to:" << fourcc(format) << " format is:" << fourcc(fmt.fmt.pix.pixelformat);
		return -1;
	}
	if ((fmt.fmt.pix.width != m_params.m_width) || (fmt.fmt.pix.height != m_params.m_height))
	{
		LOG(WARN) << "Cannot set size width:" << fmt.fmt.pix.width << " height:" << fmt.fmt.pix.height;
	}
	
	m_format     = fmt.fmt.pix.pixelformat;
	m_width      = fmt.fmt.pix.width;
	m_height     = fmt.fmt.pix.height;		
	m_bufferSize = fmt.fmt.pix.sizeimage;
	
	LOG(NOTICE) << m_params.m_devName << ":" << fourcc(m_format) << " size:" << m_params.m_width << "x" << m_params.m_height << " bufferSize:" << m_bufferSize;
	
	return 0;
}

// configure capture FPS 
int V4l2Device::configureParam(int fd)
{
	if (m_params.m_fps!=0)
	{
		struct v4l2_streamparm   param;			
		memset(&(param), 0, sizeof(param));
		param.type = m_deviceType;
		param.parm.capture.timeperframe.numerator = 1;
		param.parm.capture.timeperframe.denominator = m_params.m_fps;

		if (ioctl(fd, VIDIOC_S_PARM, &param) == -1)
		{
			LOG(WARN) << "Cannot set param for device:" << m_params.m_devName << " " << strerror(errno);
		}
	
		LOG(NOTICE) << "fps:" << param.parm.capture.timeperframe.numerator << "/" << param.parm.capture.timeperframe.denominator;
		LOG(NOTICE) << "nbBuffer:" << param.parm.capture.readbuffers;
	}
	
	XU_H264_Set_BitRate(fd,(double)m_params.m_bitrate ,m_params.m_fps);
	
	
	return 0;
}


int XU_H264_Set_BitRate(int fd, double BitRate,int m_CurrentFPS)
{	
	int ret = 0;
	int err = 0;
	__u8 ctrldata[11]={0};
	int BitRate_CtrlNum = 0;
	unsigned int chip_id = CHIP_RER9421;
	//uvc_xu_control parmeters
	__u8 xu_unit= 0; 
	__u8 xu_selector= 0;
	__u16 xu_size= 11;
	__u8 *xu_data= ctrldata;

if(chip_id == CHIP_RER9420)
	{
		xu_unit = XU_RERVISION_SYS_ID;
		xu_selector = XU_RERVISION_SYS_H264_CTRL;

		// Switch command
		xu_data[0] = 0x9A;				// Tag
		xu_data[1] = 0x03;				// H264_BitRate
	}
	else if((chip_id == CHIP_RER9421)||(chip_id == CHIP_RER9422))
	{
		xu_unit = XU_RERVISION_USR_ID;
		xu_selector = XU_RERVISION_USR_H264_CTRL;

		// Switch command
		xu_data[0] = 0x9A;				// Tag
		xu_data[1] = 0x02;				// H264_BitRate
	}
	
	if ((err=XU_Set_Cur(fd, xu_unit, xu_selector, xu_size, xu_data)) < 0) 
	{
		LOG(NOTICE) << "XU_Set_Cur err" ;
	}

	// Set Bit Rate Ctrl Number
	if(chip_id == CHIP_RER9420)
	{
		// Bit Rate = BitRate_Ctrl_Num*512*fps*8/1000 (Kbps)
		BitRate_CtrlNum = (int)((BitRate*1024)/(512*m_CurrentFPS*8));
		xu_data[0] = (BitRate_CtrlNum & 0xFF00)>>8;	// BitRate ctrl Num
		xu_data[1] = (BitRate_CtrlNum & 0x00FF);
	}
	else if((chip_id == CHIP_RER9421)||(chip_id == CHIP_RER9422))
	{
		// Bit Rate = BitRate_Ctrl_Num*512*fps*8/1000 (Kbps)
		xu_data[0] = ((int)BitRate & 0x00FF0000)>>16;
		xu_data[1] = ((int)BitRate & 0x0000FF00)>>8;
		xu_data[2] = ((int)BitRate & 0x000000FF);
	}	
	
	if ((err=XU_Set_Cur(fd, xu_unit, xu_selector, xu_size, xu_data)) < 0) 
	{
		LOG(NOTICE) << "XU_Set_Cur err" ;
	}
	
	return ret;
}

int XU_Set_Cur(int fd, __u8 xu_unit, __u8 xu_selector, __u16 xu_size, __u8 *xu_data)
{
	int err=0;
	struct uvc_xu_control_query xctrl;
	xctrl.unit = xu_unit;
	xctrl.selector = xu_selector;
	xctrl.query = UVC_SET_CUR;
	xctrl.size = xu_size;
	xctrl.data = xu_data;
	err=ioctl(fd, UVCIOC_CTRL_QUERY, &xctrl);
	return err;
}

